#include "optitopo.h"

using namespace std;
using namespace Eigen;

/**
 * @brief Construct a new optitopo::optitopo object
 * @param dim 粒子位置的维数
 * @param results_dim 待输出结果的维数
 */
optitopo::optitopo(int dim, int particle_num, int results_dim)
{
    dim_ = dim;
    particle_num_ = particle_num;
    curr_iter_ = 0;
    dt_ = new double[dim_];
    wstart_ = new double[dim_];
	wend_ = new double[dim_];
	C1_ = new double[dim_];
	C2_ = new double[dim_];
    upper_bound_ = new double[dim_];
	lower_bound_ = new double[dim_];
	range_interval_ = new double[dim_];
	particles_ = new Particle[particle_num_];
	w_ = new double[dim_];
	all_best_position_ = new double[dim_];

	results_dim_ = results_dim;

	if (results_dim_)
	{
		results_ = new double[results_dim_];
        cout << "the result will be recorded" << endl;
    }
}

// 析构函数
optitopo::~optitopo()
{
	if (particles_) { delete[]particles_; }
	if (upper_bound_) { delete[]upper_bound_; }
	if (lower_bound_) { delete[]lower_bound_; }
	if (range_interval_) { delete[]range_interval_; }
	if (dt_) { delete[]dt_; }
	if (wstart_) { delete[]wstart_; }
	if (wend_) { delete[]wend_; }
	if (w_) { delete[]w_; }
	if (C1_) { delete[]C1_; }
	if (C2_) { delete[]C2_; }
	if (all_best_position_) { delete[]all_best_position_; }
	if (results_) { delete[]results_; }
}



/**
 * @brief 利用求根公式求取拉普拉斯矩阵的次小特征值
 * @param Laplace 
 * @return double 
 */
double optitopo::SolveLambda2_Analytical(Matrix4d Laplace){
    double a = -Laplace(0, 1);
    double b = -Laplace(0, 2);
    double c = -Laplace(0, 3);
    double d = -Laplace(1, 2);
    double e = -Laplace(1, 3);
    double f = -Laplace(2, 3);
    complex<double> W(-0.5, sqrt(3) / 2);
    double A = -1;
    double B = 2 * a + 2 * b + 2 * c + 2 * d + 2 * e + 2 * f;
    double C = - 3 * a * b - 3 * a * c - 3 * a * d - 3 * b * c - 3 * a * e - 3 * b * d - 4 * a * f - 4 * b * e 
               - 4 * c * d - 3 * b * f - 3 * c * e - 3 * c * f - 3 * d * e - 3 * d * f - 3 * e * f;
    double D = 4 * a * b * c + 4 * a * b * e + 4 * a * c * d + 4 * a * b * f + 4 * b * c * d + 4 * a * c * f + 4 * a * d * e + 4 * b * c * e 
             + 4 * a * d * f + 4 * b * d * e + 4 * a * e * f + 4 * b * d * f + 4 * c * d * e + 4 * b * e * f + 4 * c * d * f + 4 * c * e * f;
    double P = (3 * A * C - B * B) / (3 * A * A);
    double Q = (27 * A * A * D - 9 * A * B * C + 2 * B * B * B) / (27 * A * A * A);
    complex<double> DELTA = pow(Q / 2, 2) + pow(P / 3, 3);  //DELTA 必须被定义为complex<double>，才能正常进行开方、开立方操作
    complex<double> DELTA_1 = pow(-Q / 2 + pow(DELTA, 1.0 / 2), 1.0 / 3); //必须用pow(x,1.0/5),才能完成开方操作
    complex<double> DELTA_2 = pow(-Q / 2 - pow(DELTA, 1.0 / 2), 1.0 / 3);
    complex<double> x1 = DELTA_1 + DELTA_2 - B / (3 * A);
    complex<double> x2 = W * DELTA_1 + W * W * DELTA_2 - B / (3 * A);
    complex<double> x3 = W * W * DELTA_1 + W * DELTA_2 - B / (3 * A);

    //由于实对称矩阵的特征值都是实数，故此可直接比较上述三个复数的模
    double norm_x1 = abs(x1);
    double norm_x2 = abs(x2);
    double norm_x3 = abs(x3);

    double temp = (norm_x1 < norm_x2) ? norm_x1 : norm_x2;
    temp = (temp < norm_x3) ? temp : norm_x3;
    return temp;
}


/**
 * @brief 利用Eigen库求取拉普拉斯矩阵的次小特征值
 * @param Laplace 
 * @return double 
 */
double optitopo::SolveLambda2_Eigen(Matrix4d Laplace){
    EigenSolver<MatrixXd> es(Laplace);
    MatrixXd eigenvalues = es.pseudoEigenvalueMatrix();
    int length = Laplace.rows();
    double eigen_array[length];
    for(int Li = 0; Li < length; Li++){
        eigen_array[Li] = eigenvalues(Li,Li);
    }
    // cout <<"eigen"<< eigen_array[3] << "  " << eigen_array[2] << "" << eigen_array[1] << endl;
    sort(eigen_array, eigen_array + length);
    return eigen_array[1]; //取次小特征值
}


/**
 * @brief 利用幂迭代求取拉普拉斯矩阵的次小特征值
 * @param Laplace 
 * @return double 
 */
double optitopo::SolveLambda2_PowerIteration(Matrix4d Laplace, int iter_times){
    Vector4d eigenvector_1(0.5, 0.5, 0.5, 0.5); //对应于最小特征值的单位特征向量
    double k = 0.25;//由于对角线的迹小于4，故特征值之和小于4，改系数可选取4 TODO
    Matrix4d tempL = Matrix4d::Identity() - k * Laplace - eigenvector_1 * eigenvector_1.transpose();
    Vector4d tempV(1, 2, 1, 1);
    for (int i = 0; i < iter_times;i++){
        tempV = tempL * tempV.normalized();
    }
    return (1 - tempV.norm()) / k;
}


/**
 * @brief 输出此时的权重
 * @param theta_i 观测者位置角
 * @param theta_j 被观测者位置角
 * @param alpha_i 观测者相机角
 * @return double 
 */
double optitopo::GetWeight(double theta_i, double theta_j, double alpha_i){
    // double dis_min = 1/2; //最小可视距离
    // double dis_max = 2;//最大可视距离
    // double D_theta_tor = 0.05; //最小角度经验误差
    // double Weight_disisfar = 0.5; //无人机位于最远距离时的距离权重
    // double Exponent = 200;
    // double CoefficientDis = 0.065; //用于调整距离权重的系数
    //用于调整角度权重的系数，取决于相机的fov，此处fov=pi/2；该系数的调整需要保证即使
    //在相机视野外，仍有权重，不然一旦上一个目标脱离相机视野，相机的规划就会显得漫无目的
    double CoefficientFov = 0.2; 
    //有别于上一个系数，该系数着重考虑目标是否在相机运动范围内的权重
    double CoefficientTheta_exp = 6;
    double CoefficientTheta_thr = M_PI*5/6;

    //目标在相机视野内的角度权重
    double theta_ij = atan2(sin(theta_j) - sin(theta_i), cos(theta_j) - cos(theta_i));
    double Weight_fov = exp(-CoefficientFov * pow((theta_ij - alpha_i),2));
    //目标在相机运动范围内的角度权重
    double Weight_theta = 1/(pow((theta_ij/CoefficientTheta_thr),CoefficientTheta_exp)+1);
    // //距离权重
    // double D_theta=theta_i-theta_j;
    // double D_theta_min=2*asin(dis_min/2);
    // double Weight_dis = (CoefficientDis * pow((abs(D_theta) - M_PI), 2) + Weight_disisfar) 
    //                     / (pow(((abs(D_theta) - M_PI) / (M_PI - D_theta_min - D_theta_tor)), Exponent) + 1);

    //总权重
    // Weight = Weight_fov * Weight_theta * Weight_dis;
    double Weight = Weight_fov * Weight_theta;
    return Weight;
}


/**
 * @brief 计算拉普拉斯矩阵的次小特征值
 * @param theta 飞机的位置角
 * @param alpha 相机的偏航角
 * @return true 
 * @return false 
 */
bool optitopo::GetLambda2(std::vector<double> theta, std::vector<double> alpha){
    int length = theta.size();
    MatrixXd Laplace = MatrixXd::Zero(length,length);

    //赋权拉普拉斯矩阵
    // //选项1.这是能同时观测视野里所有无人机的
    // for (int Li = 0; Li < length;Li++){
    //     double RowWeight = 0.0;
    //     for (int Lj = 0; Lj < length; Lj++)
    //     {
    //         if(Li==Lj){
    //             continue;
    //         }
    //         double SinglrWeight = optitopo::GetWeight(theta[Li], theta[Lj], alpha[Li]);
    //         Laplace(Li, Lj) = -SinglrWeight;
    //         RowWeight += SinglrWeight;
    //     }
    //     Laplace(Li,Li) = RowWeight;
    // }
    
    //选项2.只能观测一架无人机，且观测权重最大的一架
    for (int Li = 0; Li < length;Li++){
        double max_weight = -1000.0;
        int max_index = 0;
        for (int Lj = 0; Lj < length; Lj++)
        {
            if(Li==Lj){
                continue;
            }
            else{
                double temp_weight = optitopo::GetWeight(theta[Li], theta[Lj], alpha[Li]);
                if(temp_weight>max_weight){
                    max_weight = temp_weight;
                    max_index = Lj;
                }
            }
        }
        // //有向图
        // Laplace(Li, Li) = max_weight;
        // Laplace(Li, max_index) = -max_weight;

        //无向图 非对角元素赋值部分
        Laplace(Li,Li)=0;
        Laplace(Li,max_index) = Laplace(Li,max_index) - max_weight;
        Laplace(max_index,Li) = Laplace(max_index,Li) - max_weight;
        this->SeeWho += 10 ^ (Li - 1) * max_index;
    }
    //无向图 对角元素赋值部分
    VectorXd histogram_row = Laplace.rowwise().sum();
    for(int Li = 0; Li < length; Li++){
        Laplace(Li, Li) = -histogram_row(Li);
    }
    // cout << "Laplace" << Laplace << endl;
    //求迹
    this->TraceofL = Laplace.trace();
    //求次小特征值
    this->lambda2 = this->SolveLambda2_Analytical(Laplace); //取次小特征值
    return true;
}



/**
 * @brief 
 * @param theta 无人机的位置
 * @param d_theta 无人机的速度
 * @param alpha 相机的偏航角
 * @return true 
 * @return false 
 */
bool optitopo::GetOutput_search(std::vector<double> theta, std::vector<double> d_theta, std::vector<double> alpha, double Delta_t){
    //基本定义
    Delta_t_ = Delta_t;
    //无人机参数
    int N=theta.size();//无人机数量
    double T_exp = T_exp_;          //无人机的期望的运动周期，单位：s       
    double v_theta_exp = 2 * M_PI/T_exp; //无人机期望运动角速度，单位：rad/s
    double v_theta_max = k_v_theta_max_ * v_theta_exp; //允许无人机最大运动角速度，单位：rad/s
    double v_theta_min = k_v_theta_min_ * v_theta_exp; //无人机不能逆转,不能不走 TODO
    double alpha_max = M_PI * 5 / 6; //相机角度上限，单位：rad；
    double alpha_min = -alpha_max; //相机角度下限，单位：rad；
    double v_alpha_max = 4 * M_PI / 3; //相机运动角速度上限，实际最大角速度为 100PI/61， 单位：rad/s
    double v_alpha_min = -v_alpha_max; //相机运动角速度下限，单位：rad/s

    double a_theta_max = a_theta_max_;//无人机最大运动角加速度，单位：rad/s^2
    double a_theta_min = -a_theta_max;//无人机最小运动角加速度，单位：rad/s^2
    double dis_min = dis_min_; //两架无人机之间最小距离，单位：m
    double Delta_theta_tor = 0.1; //最小距离的余量，单位：rad
    double Delta_theta_min = 2 * asin(dis_min/2) + Delta_theta_tor;//两架无人机之间最小角度

    double Coeff_Vel = Coeff_Vel_;
    int Resolution = Resolution_;

    double temp_theta_max;
    double temp_theta_min;
    MatrixXd Temp_dd_theta = MatrixXd::Zero(N, Resolution);
    MatrixXd Temp_d_theta = MatrixXd::Zero(N, Resolution);
    MatrixXd Temp_theta = MatrixXd::Zero(N, Resolution);
    MatrixXd Temp_d_alpha = MatrixXd::Zero(N, Resolution);
    MatrixXd Temp_alpha = MatrixXd::Zero(N, Resolution);
    std::vector<double> pre_theta(N);
    for (int uavi = 0; uavi < N; uavi++)
    {
        //无人机
        //考虑与前一架飞机的安全距离，允许的最大位置增量
        if (theta[(uavi+1)%N] > theta[uavi])
            temp_theta_max = theta[(uavi+1)%N] - theta[uavi] - Delta_theta_min; 
        else
            temp_theta_max = 2*M_PI + theta[(uavi+1)%N] - theta[uavi] - Delta_theta_min;
        //考虑与后一架飞机的安全距离，允许的最大位置减量
        if (theta[(uavi+3)%N] < theta[uavi])
            temp_theta_min = theta[(uavi+3)%N] - theta[uavi] + Delta_theta_min;
        else
            temp_theta_min = -2*M_PI + theta[(uavi+3)%N] - theta[uavi] + Delta_theta_min;
        //仅考虑位置限制的无人机最大加速度,假定前后无人机速度不变
        double temp_dd_theta_max;
        double temp_dd_theta_min;
        if (temp_theta_min < temp_theta_max)
        {
            double temp_dd_theta_max_fromp = 2 * (temp_theta_max - (d_theta[uavi] - d_theta[(uavi + 1) % N]) * Delta_t_) / pow(Delta_t_, 2);
            double temp_dd_theta_min_fromp = 2 * (temp_theta_min - (d_theta[uavi] - d_theta[(uavi + 3) % N]) * Delta_t_) / pow(Delta_t_, 2);
            //仅考虑速度限制的无人机最大加速度
            double temp_dd_theta_max_fromv = (v_theta_max-d_theta[uavi])/Delta_t_;
            double temp_dd_theta_min_fromv = (v_theta_min-d_theta[uavi])/Delta_t_;
            //取 无人机本身最大加速度 与 仅考虑速度限制的最大加速度 中的较小值
            temp_dd_theta_max = min({temp_dd_theta_max_fromp,temp_dd_theta_max_fromv,a_theta_max});
            temp_dd_theta_min = max({temp_dd_theta_min_fromp,temp_dd_theta_min_fromv,a_theta_min});
        }
        else{
            temp_dd_theta_max = 0;
            temp_dd_theta_min = 0;
            cout<<"temp_dd_theta_max<temp_dd_theta_min!!!!!!!!!!!!!!!!!!!!"<<endl;
        }
        for (int i_reso = 0; i_reso < Resolution;i_reso++){
            //等间隔采样
            Temp_dd_theta(uavi, i_reso) = temp_dd_theta_min + (temp_dd_theta_max - temp_dd_theta_min) * i_reso / (Resolution - 1);
            Temp_d_theta(uavi, i_reso)=Delta_t_ * Temp_dd_theta(uavi, i_reso) + d_theta[uavi];
            Temp_theta(uavi,i_reso) = 0.5 * pow(Delta_t_,2) * Temp_dd_theta(uavi,i_reso) + Delta_t_ * d_theta[uavi] + theta[uavi];
        }

        //相机
        //仅考虑位置限制的相机最大速度
        double temp_d_alpha_max_fromv = (alpha_max-alpha[uavi])/Delta_t_;
        double temp_d_alpha_min_fromv = (alpha_min-alpha[uavi])/Delta_t_;
        //取 相机本身最大速度 与 仅考虑位置限制的最大速度中的较小值
        double temp_d_alpha_max = min({temp_d_alpha_max_fromv, v_alpha_max});
        double temp_d_alpha_min = max({temp_d_alpha_min_fromv, v_alpha_min});
        // cout << "temp_d_alpha_max" << temp_d_alpha_max << endl;
        // cout << "temp_d_alpha_min" << temp_d_alpha_min << endl;
        for (int i_reso = 0; i_reso < Resolution; i_reso++)
        {
            Temp_d_alpha(uavi, i_reso) = temp_d_alpha_min + (temp_d_alpha_max - temp_d_alpha_min) * i_reso / (Resolution - 1);
            Temp_alpha(uavi,i_reso) = Delta_t_ * Temp_d_alpha(uavi, i_reso) + alpha[uavi];
            // cout << "Temp_d_alpha(uavi, i_reso)" << Temp_d_alpha(uavi, i_reso) << endl;
            // cout << "Temp_alpha(uavi,i_reso)" << Temp_alpha(uavi,i_reso) << endl;
        }
        //预测下一时刻的无人机位置
        pre_theta[uavi]=theta[uavi]+ d_theta[uavi] * Delta_t_;
    }

    //在预测的无人机位置（pre_theta）下，搜索最优相机位置（alpha）
    std::vector<double> alpha_input(4);  //此处指定了数量，不太合适
    std::vector<double> alpha_output(4);
    double lambda2_max = -1000;
    int index_lambda2_max[4];
    for (int var0 = 0; var0 < Resolution; var0++){
        for (int var1 = 0; var1 < Resolution; var1++){
            for (int var2 = 0; var2 < Resolution; var2++){
                for (int var3 = 0; var3 < Resolution; var3++){
                    alpha_input[0] = Temp_alpha(0, var0);
                    alpha_input[1] = Temp_alpha(1, var1);
                    alpha_input[2] = Temp_alpha(2, var2);
                    alpha_input[3] = Temp_alpha(3, var3);

                    this->GetLambda2(pre_theta, alpha_input);
                    if (this->lambda2 > lambda2_max){
                        lambda2_max = this->lambda2;
                        index_lambda2_max[0] = var0;
                        index_lambda2_max[1] = var1;
                        index_lambda2_max[2] = var2;
                        index_lambda2_max[3] = var3;
                    }
                }
            }
        }
    }
    for (int i = 0; i < 4; i++)
    {
        this->alpha_out(i) = Temp_alpha(i, index_lambda2_max[i]);
        this->d_alpha_out(i) = Temp_d_alpha(i, index_lambda2_max[i]);
        alpha_output[i] = Temp_alpha(i, index_lambda2_max[i]);
    }

    //在最优相机位置（alpha）下，搜索最优无人机位置（theta）
    std::vector<double> theta_input(4);  //此处指定了数量，不太合适
    std::vector<double> d_theta_input(4);
    lambda2_max = -1000;
    for (int var0 = 0; var0 < Resolution; var0++){
        for (int var1 = 0; var1 < Resolution; var1++){
            for (int var2 = 0; var2 < Resolution; var2++){
                for (int var3 = 0; var3 < Resolution; var3++){
                    theta_input[0] = Temp_theta(0, var0);
                    theta_input[1] = Temp_theta(1, var1);
                    theta_input[2] = Temp_theta(2, var2);
                    theta_input[3] = Temp_theta(3, var3);
                    d_theta_input[0] = Temp_d_theta(0, var0);
                    d_theta_input[1] = Temp_d_theta(1, var1);
                    d_theta_input[2] = Temp_d_theta(2, var2);
                    d_theta_input[3] = Temp_d_theta(3, var3);
                    double Total_delta_theta = 0;
                    for (int theta_i = 0; theta_i < N; theta_i++){
                        Total_delta_theta += abs(d_theta_input[theta_i] - v_theta_exp);
                    }
                    this->GetLambda2(theta_input, alpha_output);
                    //计算速度权重
                    // double Weight_Vel = 1 - Coeff_Vel * Total_delta_theta/(v_theta_exp * 4);
                    double Weight_Vel = exp(- Coeff_Vel * Total_delta_theta/(v_theta_exp * 4));
                    // std::cout << "Weight_Vel" << Weight_Vel << std::endl;
                    double lambda2withvw = Weight_Vel * this->lambda2;
                    if (lambda2withvw > lambda2_max)
                    {
                        lambda2_max = lambda2withvw;
                        index_lambda2_max[0] = var0;
                        index_lambda2_max[1] = var1;
                        index_lambda2_max[2] = var2;
                        index_lambda2_max[3] = var3;
                    }
                }
            }
        }
    }
    // std::cout << "lambda2_max" << lambda2_max << std::endl;
    for (int i = 0; i < 4; i++)
    {
        double temp_theta_ = Temp_theta(i, index_lambda2_max[i]);
        this->theta_out(i) = (temp_theta_ < 2 * M_PI) ? temp_theta_ : (temp_theta_ - 2 * M_PI);
        this->d_theta_out(i) = Temp_d_theta(i, index_lambda2_max[i]);
        this->dd_theta_out(i) = Temp_dd_theta(i, index_lambda2_max[i]);
    }
    return true;
}


bool optitopo::GetOutput_pso(std::vector<double> theta, std::vector<double> d_theta, std::vector<double> alpha, double Delta_t){
    //基本定义
    Delta_t_ = Delta_t;
    //无人机参数
    int N=theta.size();//无人机数量
    double T_exp = T_exp_;          //无人机的期望的运动周期，单位：s       
    double v_theta_exp = 2 * M_PI/T_exp; //无人机期望运动角速度，单位：rad/s
    v_theta_exp_ = v_theta_exp;
    double v_theta_max = k_v_theta_max_ * v_theta_exp; //允许无人机最大运动角速度，单位：rad/s
    double v_theta_min = k_v_theta_min_ * v_theta_exp; //无人机不能逆转,不能不走 TODO
    double alpha_max = M_PI * 5 / 6; //相机角度上限，单位：rad；
    double alpha_min = -alpha_max; //相机角度下限，单位：rad；
    double v_alpha_max = 4 * M_PI / 3; //相机运动角速度上限，实际最大角速度为 100PI/61， 单位：rad/s
    double v_alpha_min = -v_alpha_max; //相机运动角速度下限，单位：rad/s

    double a_theta_max = a_theta_max_;//无人机最大运动角加速度，单位：rad/s^2
    double a_theta_min = -a_theta_max;//无人机最小运动角加速度，单位：rad/s^2
    double dis_min = dis_min_; //两架无人机之间最小距离，单位：m
    double Delta_theta_tor = 0.1; //最小距离的余量，单位：rad
    double Delta_theta_min = 2 * asin(dis_min/2) + Delta_theta_tor;//两架无人机之间最小角度

    double Coeff_Vel = Coeff_Vel_;

    // 给类的内部成员赋值，用于计算粒子适应度
    for (int i = 0; i < N;i++){
        alpha_in_(i) = alpha[i];
        theta_in_(i) = theta[i];
        d_theta_in_(i) = d_theta[i];
    }

    double temp_theta_max;
    double temp_theta_min;
    // 相机偏角搜索过程
    Iter_Type_ = 0;
    std::srand((unsigned int)time(0));
    for (int uavi = 0; uavi < N; uavi++)
    {
        //仅考虑位置限制的相机最大速度
        double temp_d_alpha_max_fromv = (alpha_max-alpha[uavi])/Delta_t_;
        double temp_d_alpha_min_fromv = (alpha_min-alpha[uavi])/Delta_t_;
        //取 相机本身最大速度 与 仅考虑位置限制的最大速度中的较小值
        double temp_d_alpha_max = min({temp_d_alpha_max_fromv, v_alpha_max});
        double temp_d_alpha_min = max({temp_d_alpha_min_fromv, v_alpha_min});

        //预测下一时刻的无人机位置
        pre_theta_(uavi)=theta[uavi]+ d_theta[uavi] * Delta_t_;

        //赋值给粒子
        if (temp_d_alpha_min < temp_d_alpha_max)
        {
            lower_bound_[uavi] = temp_d_alpha_min;
            upper_bound_[uavi] = temp_d_alpha_max;
        }
        else{
            lower_bound_[uavi] = -0.01;
            upper_bound_[uavi] = 0.01;
            cout << "temp_d_alpha_max<temp_d_alpha_min!!!!!!!!!!!!!!!!!!!!" << endl;
        }
        range_interval_[uavi] = temp_d_alpha_max - temp_d_alpha_min;
    }
    InitialAllParticles();
    GetFinalResult();
    for(int uavi = 0; uavi < N; uavi++){
        d_alpha_out(uavi) = all_best_position_[uavi];
        alpha_out(uavi) = all_best_position_[uavi] * Delta_t_ + alpha_in_(uavi);
    }

    // 无人机位置搜索过程
    Iter_Type_ = 1;
    for (int uavi = 0; uavi < N; uavi++)
    {
        //无人机
        //考虑与前一架飞机的安全距离，允许的最大位置增量
        if (theta[(uavi+1)%N] > theta[uavi])
            temp_theta_max = theta[(uavi+1)%N] - theta[uavi] - Delta_theta_min; 
        else
            temp_theta_max = 2*M_PI + theta[(uavi+1)%N] - theta[uavi] - Delta_theta_min;
        //考虑与后一架飞机的安全距离，允许的最大位置减量
        if (theta[(uavi+3)%N] < theta[uavi])
            temp_theta_min = theta[(uavi+3)%N] - theta[uavi] + Delta_theta_min;
        else
            temp_theta_min = -2*M_PI + theta[(uavi+3)%N] - theta[uavi] + Delta_theta_min;
        // cout << "temp_theta_max" << temp_theta_max << endl;
        // cout << "temp_theta_min" << temp_theta_min << endl;
        
        double temp_dd_theta_max;
        double temp_dd_theta_min;
        if (temp_theta_min < temp_theta_max)
        {
            //仅考虑位置限制的无人机最大加速度,假定前后无人机速度不变
            double temp_dd_theta_max_fromp = 2 * (temp_theta_max - (d_theta[uavi] - d_theta[(uavi + 1) % N]) * Delta_t_) / pow(Delta_t_, 2);
            double temp_dd_theta_min_fromp = 2 * (temp_theta_min - (d_theta[uavi] - d_theta[(uavi + 3) % N]) * Delta_t_) / pow(Delta_t_, 2);
            
            //仅考虑速度限制的无人机最大加速度
            double temp_dd_theta_max_fromv = (v_theta_max-d_theta[uavi])/Delta_t_;
            double temp_dd_theta_min_fromv = (v_theta_min-d_theta[uavi])/Delta_t_;
            temp_dd_theta_max_fromp = (temp_dd_theta_max_fromp > a_theta_min) ? temp_dd_theta_max_fromp : (a_theta_min / 2);
            temp_dd_theta_max_fromv = (temp_dd_theta_max_fromv > a_theta_min) ? temp_dd_theta_max_fromv : (a_theta_min / 2);
            temp_dd_theta_min_fromp = (temp_dd_theta_min_fromp < a_theta_max) ? temp_dd_theta_min_fromp : (a_theta_max / 2);
            temp_dd_theta_min_fromv = (temp_dd_theta_min_fromv < a_theta_max) ? temp_dd_theta_min_fromv : (a_theta_max / 2);
            // if(uavi == 0){
            //     cout << "temp_dd_theta_max_fromp" << temp_dd_theta_max_fromp << endl;
            //     cout << "temp_dd_theta_min_fromp" << temp_dd_theta_min_fromp << endl;
            //     cout << "temp_dd_theta_max_fromv" << temp_dd_theta_max_fromv << endl;
            //     cout << "temp_dd_theta_min_fromv" << temp_dd_theta_min_fromv << endl;
            // }

            //取 无人机本身最大加速度 与 仅考虑速度限制的最大加速度 中的较小值
            temp_dd_theta_max = min({temp_dd_theta_max_fromp,temp_dd_theta_max_fromv,a_theta_max});
            temp_dd_theta_min = max({temp_dd_theta_min_fromp,temp_dd_theta_min_fromv,a_theta_min});
        }
        else{ // TODO
            temp_dd_theta_max = 0;
            temp_dd_theta_min = 0;
            cout << "temp_dd_theta_max<temp_dd_theta_min!!!!!!!!!!!!!!!!!!!!" << endl;
        }
        // cout << "temp_dd_theta_min" << temp_dd_theta_min << endl;
        // cout << "temp_dd_theta_max" << temp_dd_theta_max << endl;
        if (temp_dd_theta_min < temp_dd_theta_max)
        {
            lower_bound_[uavi] = temp_dd_theta_min;
            upper_bound_[uavi] = temp_dd_theta_max;
        }
        else{
            lower_bound_[uavi] = temp_dd_theta_max-0.1;
            upper_bound_[uavi] = temp_dd_theta_max;
            cout << "temp_dd_theta_max<temp_dd_theta_min???????????????????" << endl;
        }
        range_interval_[uavi] = temp_dd_theta_max - temp_dd_theta_min;
        cout << "range_interval_" << range_interval_[uavi] << endl;
    }

    InitialAllParticles();
    GetFinalResult();

    for(int uavi = 0; uavi < N; uavi++){
        dd_theta_out(uavi) = all_best_position_[uavi];
        d_theta_out(uavi) = d_theta_in_(uavi) + dd_theta_out(uavi) * Delta_t_;
        double temp_theta = theta_in_(uavi) + d_theta_in_(uavi) * Delta_t_ + 1 / 2 * dd_theta_out(uavi) * pow(Delta_t_, 2);
        theta_out(uavi) = (temp_theta < 2 * M_PI) ? temp_theta : (temp_theta - 2 * M_PI);
    }

    return true;
}


/**
 * @brief 初始化所有粒子
 */
void optitopo::InitialAllParticles()
{
    curr_iter_ = 0;
	// 初始化第一个粒子参数并设置最优值
	InitialParticle(0);
	all_best_fitness_ = particles_[0].best_fitness_;
	for (int j = 0; j < dim_; j++)
	{
		all_best_position_[j] = particles_[0].best_position_[j];
	}

	// 初始化其他粒子，并更新最优值
	for (int i = 1; i < particle_num_; i++)
	{
		InitialParticle(i);
#ifdef MAXIMIZE_FITNESS
		if (particles_[i].best_fitness_ > all_best_fitness_)
#else
		if (particles_[i].best_fitness_ < all_best_fitness_)
#endif
		{
			all_best_fitness_ = particles_[i].best_fitness_;
			for (int j = 0; j < dim_; j++)
			{
				all_best_position_[j] = particles_[i].best_position_[j];
			}

			// // 如果需要保存出一些结果 TODO
			// if (particles_[i].results_dim_)
			// {
			// 	for (int k = 0; k < results_dim_; k++)
			// 	{
			// 		results_[k] = particles_[i].results_[k];
			// 	}
			// }
			// else if (results_dim_)
			// {
			// 	std::cout << "WARNING: the dimension of your saved results for every particle\nis not match with the dimension you specified for PSO optimizer ant no result is saved!" << std::endl;
			// }
		}
	}
}


/**
 * @brief 获取双精度随机数（默认精度为0.0001）
 * @param N 精度倒数
 * @return double 
 */
double optitopo::GetDoubleRand(int N)
{
	double temp = rand() % (N + 1) / (double)(N + 1);
	return temp;
}


/**
 * @brief 完成设定次数的迭代过程
 */
void optitopo::GetFinalResult(){
	for (int i = 0; i < max_iter_num_; i++)
	{
		UpdateAllParticles();
		// result[i] = psooptimizer.all_best_fitness_;
		// std::cout << "第" << i << "次迭代结果：";
		// std::cout << "x = " << psooptimizer.all_best_position_[0] << ", " << "y = " << psooptimizer.all_best_position_[1];
		// std::cout << ", fitness = " << result[i] << std::endl;
	}
}


/**
 * @brief 更新所有粒子参数
 */
void optitopo::UpdateAllParticles()
{
	GetInertialWeight();
	for (int i = 0; i < particle_num_; i++)
	{
		UpdateParticle(i);
#ifdef MAXIMIZE_FITNESS
		if (particles_[i].best_fitness_ > all_best_fitness_)
#else
		if (particles_[i].best_fitness_ < all_best_fitness_)
#endif
		{
			all_best_fitness_ = particles_[i].best_fitness_;
			for (int j = 0; j < dim_; j++)
			{
				all_best_position_[j] = particles_[i].best_position_[j];
			}
			
			// // 如果需要保存出一些参数
			// if (particles_[i].results_dim_ && results_dim_ == particles_[i].results_dim_)
			// {
			// 	for (int k = 0; k < results_dim_; k++)
			// 	{
			// 		results_[k] = particles_[i].results_[k];
			// 	}
			// }
			// else if (results_dim_)
			// {
			// 	std::cout << "WARNING: the dimension of your saved results for every particle\nis not match with the dimension you specified for PSO optimizer ant no result is saved!" << std::endl;
			// }
		}
	}
	curr_iter_++;
}


/**
 * @brief 更新第i个粒子
 * @param i 
 */
void optitopo::UpdateParticle(int i)
{
	// 计算当前迭代的权重
	for (int j = 0; j < dim_; j++)
	{
		// 保存上一次迭代结果的position和velocity
		//double last_velocity = particles_[i].velocity_[j];
		double last_position = particles_[i].position_[j];

		particles_[i].velocity_[j] = w_[j] * particles_[i].velocity_[j] +
			C1_[j] * GetDoubleRand() * (particles_[i].best_position_[j] - particles_[i].position_[j]) +
			C2_[j] * GetDoubleRand() * (all_best_position_[j] - particles_[i].position_[j]);
		particles_[i].position_[j] += dt_[j] * particles_[i].velocity_[j];

		// 如果搜索区间有上下限限制
		if (upper_bound_ && lower_bound_)
		{
			if (particles_[i].position_[j] > upper_bound_[j])
			{
				double thre = GetDoubleRand(99);
				if (last_position == upper_bound_[j])
				{
					particles_[i].position_[j] = GetDoubleRand() * range_interval_[j] + lower_bound_[j];
				}
				else if (thre < 0.5)
				{
					particles_[i].position_[j] = upper_bound_[j] - (upper_bound_[j] - last_position) * GetDoubleRand();
				}
				else
				{
					particles_[i].position_[j] = upper_bound_[j];
				}		
			}
			if (particles_[i].position_[j] < lower_bound_[j])
			{
				double thre = GetDoubleRand(99);
				if (last_position == lower_bound_[j])
				{
					particles_[i].position_[j] = GetDoubleRand() * range_interval_[j] + lower_bound_[j];
				}
				else if (thre < 0.5)
				{
					particles_[i].position_[j] = lower_bound_[j] + (last_position - lower_bound_[j]) * GetDoubleRand();
				}
				else
				{
					particles_[i].position_[j] = lower_bound_[j];
				}
			}
		}
	}

	if(Iter_Type_==0){
        particles_[i].fitness_ = GetFitness_Camera(particles_[i]);
    }
    else{
        particles_[i].fitness_ = GetFitness_Uav(particles_[i]);
    }

#ifdef MAXIMIZE_FITNESS
	if (particles_[i].fitness_ > particles_[i].best_fitness_)
#else
	if (particles_[i].fitness_ < particles_[i].best_fitness_)
#endif
	{
		particles_[i].best_fitness_ = particles_[i].fitness_;
		for (int j = 0; j < dim_; j++)
		{
			particles_[i].best_position_[j] = particles_[i].position_[j];
		}
	}
}


/**
 * @brief 获取当前迭代的权重
 */
void optitopo::GetInertialWeight()
{
	double temp = curr_iter_ / (double)max_iter_num_;
	temp *= temp; // TODO
	for (int i = 0; i < dim_; i++)
	{
		w_[i] = wstart_[i] - (wstart_[i] - wend_[i]) * temp;
	}
}


/**
 * @brief 初始化第i个粒子参数
 * @param i 
 */
void optitopo::InitialParticle(int i)
{
	// 为每个粒子动态分配内存
	particles_[i].position_ = new double[dim_];
	particles_[i].velocity_ = new double[dim_];
	particles_[i].best_position_ = new double[dim_];

	//if (results_dim_)
	//{
	//	particles_[i].results_ = new double[results_dim_];
	//}

	// 初始化position/veloctiy值
	for (int j = 0; j < dim_; j++)
	{
		// if defines lower bound and upper bound
		if (range_interval_)
		{
			particles_[i].position_[j] = GetDoubleRand() * range_interval_[j] + lower_bound_[j];
			particles_[i].velocity_[j] = GetDoubleRand() * range_interval_[j] / 300;
			//std::cout << particles_[i].position_[j] << std::endl;
		}
		else
		{
			particles_[i].position_[j] = GetDoubleRand() * 2;
			particles_[i].velocity_[j] = GetDoubleRand() * 0.5;
		}
	}

	// 设置初始化最优适应度值
    if(Iter_Type_==0){
        particles_[i].fitness_ = GetFitness_Camera(particles_[i]);
    }
    else{
        particles_[i].fitness_ = GetFitness_Uav(particles_[i]);
    }

	for (int j = 0; j < dim_; j++)
	{
		particles_[i].best_position_[j] = particles_[i].position_[j];
	}
	particles_[i].best_fitness_ = particles_[i].fitness_;
}


double optitopo::GetFitness_Camera(Particle& particle){
    std::vector<double> alpha_in(4);
    std::vector<double> theta_in(4);
    for (int i = 0; i < dim_; i++)
    {
        alpha_in[i] = Delta_t_ * particle.position_[i] + alpha_in_(i);
        theta_in[i] = pre_theta_(i);
    }
    GetLambda2(theta_in, alpha_in);
    return lambda2;
}

double optitopo::GetFitness_Uav(Particle& particle){
    std::vector<double> alpha_in(4);
    std::vector<double> theta_in(4);
    double Total_delta_theta = 0;
    for (int i = 0; i < dim_; i++)
    {
        alpha_in[i] = alpha_out(i);
        theta_in[i] = 1 / 2 * particle.position_[i] * pow(Delta_t_, 2) + d_theta_in_(i) * Delta_t_ + theta_in_(i);
        double vel_cur = d_theta_in_(i) + particle.position_[i] * Delta_t_;
        Total_delta_theta += abs(vel_cur - v_theta_exp_);
    }
    double Weight_Vel = exp(- Coeff_Vel_ * Total_delta_theta/(v_theta_exp_ * 4));
    return Weight_Vel * GetLambda2(theta_in, alpha_in);
}